/* Clock in Hz */
#ifndef F_CPU
#define F_CPU 8000000UL
#endif

#include "pwm_teste.h"

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>


volatile uint8_t HR_COUNTER2; // High Register for Timer/Counter2
volatile uint16_t duty_cicle; // PWM duty cicle in us

ISR(TIMER2_OVF_vect) {
  // Increment the High Register on TCNT2 overflow
  HR_COUNTER2+=1;
}

ISR(PCINT1_vect) {

  if (bit_is_set(PINC,0)) {
    // When going high, reset counters
    HR_COUNTER2 = 0; 
    TCNT2 = 0;
  }
  else {
    // When going low, stop timer, read duty_cicle and re-start
    // counter with 8 prescaler
    TCCR2B &= (0 << CS21);
    if (TIFR2 & (1<<TOV2)) HR_COUNTER2 += 1;
    duty_cicle = ((uint16_t) HR_COUNTER2 << 8) | TCNT2;
    duty_cicle -= 1;
    TCCR2B |= _BV(CS21);
  }

}

void timer2_init(void){

  // 8 prescaler for 1MHz clock
  TCCR2B |= _BV(CS21);

  /* Enable Timer Overflow Interrupts */
  TIMSK2 |= _BV(TOIE2);

  /* Set counters to 0 */
  HR_COUNTER2 = 0;
  TCNT2 = 0;
}

void set_pin_interrupt(void){
  
  PCICR |= _BV(PCIE1);
  PCMSK1 |= _BV(PCINT8);

}

void write_pwm_init(void) {

	TCCR1A = _BV(WGM11) |  _BV(COM1A1);
	TCCR1B = _BV(CS11) | _BV(WGM12) | _BV(WGM13); /* prescaler of 8, fast PWM */

        // 3456 is the servo center position
        OCR1A = 1500; /* 125 - 250 */
        ICR1 = 19999; // that evens out to 50hz

        DDRB = _BV(PB1); /* Set pin as output */
}


void read_pwm_init(void){
  timer2_init();
  set_pin_interrupt();
}
